import rclpy
from rclpy.node import Node
# TF坐标监听器
from tf2_ros import TransformListener, Buffer

from geometry_msgs.msg import TransformStamped # TF消息接口
from tf_transformations import euler_from_quaternion # 四元数转欧拉角
# ROS_MSG使用四元数记录欧拉角信息

import math # 角度转弧度

class DynamicTFListener(Node):
    def __init__(self):
        super().__init__('dynamic_tf_listener')
        self.buffer_ = Buffer()
        self.dynamic_listener_ = TransformListener(self.buffer_, self)
        self.timer_ = self.create_timer(0.5, self.get_transform) # 定时器，每隔1秒发布一次TF坐标
       
    def get_transform(self):
        """ 获取TF坐标 """
        try:
            result = self.buffer_.lookup_transform("map", "base_footprint", 
                rclpy.time.Time(seconds=0.0), 
                rclpy.time.Duration(seconds=1.0)) # 获取TF坐标
            transform = result.transform # 获取TF坐标
            self.get_logger().info(f"平移:{transform.translation}") # 打印日志信息
            self.get_logger().info(f"旋转四元数:{transform.rotation}") # 打印日志信息
            rotation = euler_from_quaternion([transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w]) # 四元数转欧拉角
            self.get_logger().info(f"旋转欧拉角:{rotation}") # 打印日志信息

        except Exception as e:
            self.get_logger().error(f"TF坐标获取失败:{str(e)}") # 打印日志信息


def main():
    rclpy.init()
    node = DynamicTFListener()
    rclpy.spin(node)
    rclpy.shutdown()

    